#include "associate_factor.h"

#include <cmath>

#include "rclcpp/rclcpp.hpp"
auto logger2 = rclcpp::get_logger("abc");

namespace bevdet::tracker {

AssociateFactor::AssociateFactor(std::string name, int32_t weight)
    : name_(std::move(name))
    , weight_(weight)
{
}

std::string AssociateFactor::getName(void) const
{
    return name_;
}

int32_t AssociateFactor::getWeight(void) const
{
    return weight_;
}

PositionXAssociateFactor::PositionXAssociateFactor(std::string name, double weight, double dxThreshold)
    : AssociateFactor(std::move(name), weight)
    , dxThreshold_(dxThreshold)
{
}

int32_t PositionXAssociateFactor::assessScore(const Object& o1, const Object& o2) const
{
    auto dt = (int64_t)(o1.header.stamp.sec - o2.header.stamp.sec) * 1000000 + (o1.header.stamp.nanosec - o2.header.stamp.nanosec) / 1000;

    auto x = o1.x + dt * o1.speed * cos(o1.heading * M_PI / 180);

    auto dx = std::abs(x - o2.x);

    if (dx > dxThreshold_) {
        RCLCPP_INFO(logger2, "x %lld %f %f %f  %f %f ", dt, x, dx, dxThreshold_, o1.x, o2.x);
        return -1;
    }

    return (1.0 - dx / dxThreshold_) * 100;
}

PositionYAssociateFactor::PositionYAssociateFactor(std::string name, double weight, double dyThreshold)
    : AssociateFactor(std::move(name), weight)
    , dyThreshold_(dyThreshold)
{
}

int32_t PositionYAssociateFactor::assessScore(const Object& o1, const Object& o2) const
{
    auto dt = (int64_t)(o1.header.stamp.sec - o2.header.stamp.sec) * 1000000 + (o1.header.stamp.nanosec - o2.header.stamp.nanosec) / 1000;

    auto y = o1.y + dt * o1.speed * sin(o1.heading * M_PI / 180);

    auto dy = std::abs(y - o2.y);

    if (dy > dyThreshold_) {
        RCLCPP_INFO(logger2, "y %lld %f %f %f", dt, y, dy, dyThreshold_);
        return -1;
    }

    return (1.0 - dy / dyThreshold_) * 100;
}

HeadingAssociateFactor::HeadingAssociateFactor(std::string name, double weight, double headingThreshold)
    : AssociateFactor(std::move(name), weight)
    , headingThreshold_(headingThreshold)
{
}

int32_t HeadingAssociateFactor::assessScore(const Object& o1, const Object& o2) const
{
    auto headingDiff = o1.heading - o2.heading;

    if (abs(headingDiff) >= headingThreshold_) {
        RCLCPP_INFO(logger2, " %f %f %f %f", o1.heading, o2.heading, headingDiff, headingThreshold_);
        return -1;
    }

    return (1.0 - abs(headingDiff) / headingThreshold_) * 100;
}
}